/**
 * by Shen Haoyuan(hoyin) @https://gitee.com/shenhaoyuan @AUST
 */

#include <iostream>
#include <opencv2/features2d.hpp>
#include <opencv2/core.hpp>
#include "utils.h"
#include "RelativeOrientation.h"
#include "DatasetLoader.h"
#include <thread>
#include <chrono>
#include "PartiDetector.h"
#include "PartiMatcher.h"
#include "Structures/ProgramConfig.h"
#include "ObjectMatcher.h"
#include "ForwardIntersection.h"


ContinuousOrient::Result calcCO(const cv::Mat& lScene, const cv::Mat& rScene, const CameraParams &cameraParams, const ProgramConfig::FeatureParams featureParams) {

	cv::Mat descriptors1, descriptors2;
	std::vector<cv::KeyPoint> keypoints1, keypoints2;
	cv::Ptr<cv::ORB> orb = cv::ORB::create(featureParams.n_features, featureParams.scale_factor, featureParams.n_levels,
										   featureParams.patch_size, featureParams.first_level, 2, cv::ORB::HARRIS_SCORE,
										   featureParams.patch_size, 20);
//	特征提取
	PartiDetector pd(orb, featureParams.feature_blocks_h, featureParams.feature_blocks_v);
	pd.detect(lScene, keypoints1);
	pd.detect(rScene, keypoints2);
//  计算描述符
	orb->compute(lScene, keypoints1, descriptors1);
	orb->compute(rScene, keypoints2, descriptors2);
//  特征匹配
	std::vector<cv::DMatch> matches;
	cv::BFMatcher bfMatcher(cv::NORM_HAMMING);
	PartiMatcher pm(bfMatcher, featureParams.min_dist_threshold);
	pm.match(keypoints1, keypoints2, descriptors1, descriptors2, pd.getROIs(), matches, lScene, rScene, featureParams.feature_blocks_h, featureParams.feature_blocks_v);
	std::cout << " available match: " << matches.size() << std::endl;
//	相对定向
	ContinuousOrient co(cameraParams.cam0_intrinsic, cameraParams.cam0_intrinsic,
						new double[] { cameraParams.cam0_ppm_x, cameraParams.cam0_ppm_y },
						new double[] { cameraParams.cam1_ppm_x, cameraParams.cam1_ppm_y });
	co.setPointPairs(keypoints1, keypoints2, matches);
	co.setBaselineLength(cameraParams.baseline.x());
	co.solve();
	co.output();
	return co.getResult();
}


void objectRecognize(cv::Mat &lScene, cv::Mat &rScene, ProgramConfig::YOLOParams &yoloParams, std::vector<YOLOBox> &lYOLOBoxes,
					 std::vector<YOLOBox> &rYOLOBoxes, std::vector<std::pair<int, int>> &matchMap) {
	ObjectMatcher om(yoloParams.model);
	om.detectAndMatch(lScene, rScene, lYOLOBoxes, rYOLOBoxes, matchMap);
}

int main(int argc, const char* argv[]) {
//	读取启动配置
	ProgramConfig programCfg(argv[1]);

//	读取双目配置
	StereoParams stereoParams = programCfg.getStereoParams();

//	读取数据集配置
	ProgramConfig::DatasetParams datasetParams = programCfg.getDatasetParams();

//	读取特征提取配置
	ProgramConfig::FeatureParams featureParams = programCfg.getFeatureParams();

//  读取YOLO配置
	ProgramConfig::YOLOParams yoloParams = programCfg.getYOLOParams();

//	预加载数据集
	cv::Mat lScene_gry, rScene_gry, lScene_clr, rScene_clr, mask;
	ImageLoader lIml(datasetParams.image_set_0, datasetParams.cache_size);
	ImageLoader rIml(datasetParams.image_set_1, datasetParams.cache_size);
	lIml.load();
	rIml.load();

//	读取图像数据建立双像模型
	while (!lIml.ended() && !rIml.ended()) {
		if (lIml.getQueueSize() == 0 || rIml.getQueueSize() == 0) {
			std::cerr << "Loading..." << std::endl;
			std::this_thread::sleep_for(std::chrono::seconds(1));
		}
		lScene_clr = lIml.pop();
		rScene_clr = rIml.pop();
		cv::cvtColor(lScene_clr, lScene_gry, cv::COLOR_BGR2GRAY);
		cv::cvtColor(rScene_clr, rScene_gry, cv::COLOR_BGR2GRAY);
		std::cerr << "Orientating " << lIml.getOutCount() << " ~ " << rIml.getOutCount() << std::endl;
//		ContinuousOrient::Result coResult = calcCO(lScene_gry, rScene_gry, stereoParams, featureParams);
		ContinuousOrient::Result coResult = {
				Eigen::Vector3d(532.7, 0, 0),
				Eigen::Matrix3d::Identity()
		};
		std::cerr << "Object detecting " << lIml.getOutCount() << " ~ " << rIml.getOutCount() << std::endl;
		std::vector<YOLOBox> lYOLOBoxes, rYOLOBoxes;
		std::vector<std::pair<int, int>> matchMap;
		objectRecognize(lScene_clr, rScene_clr, yoloParams, lYOLOBoxes, rYOLOBoxes, matchMap);

//		Projection Matrices
		cv::Mat T1 = (cv::Mat_<float>(3, 4) <<
		        1, 0, 0, 0,
				0, 1, 0, 0,
				0, 0, 1, 0);
		cv::Mat T2 = (cv::Mat_<float>(3, 4) <<
		        coResult.RotateMatrix(0, 0), coResult.RotateMatrix(0, 1), coResult.RotateMatrix(0, 2), coResult.RStation(0)*0.001,
		        coResult.RotateMatrix(1, 0), coResult.RotateMatrix(1, 1), coResult.RotateMatrix(1, 2), coResult.RStation(1)*0.001,
		        coResult.RotateMatrix(2, 0), coResult.RotateMatrix(2, 1), coResult.RotateMatrix(2, 2), coResult.RStation(2)*0.001);

		cv::Mat canvas = lScene_clr.clone();
		cv::line(canvas, cv::Point(0, canvas.rows/2), cv::Point(canvas.cols-1, canvas.rows/2), cv::Scalar(0,0,220));
		cv::line(canvas, cv::Point(canvas.cols/2, 0), cv::Point(canvas.cols/2, canvas.rows-1), cv::Scalar(0,0,220));
		cv::circle(canvas, cv::Point2d(stereoParams.cam0_px*stereoParams.cam0_ppm_x, stereoParams.cam0_py*stereoParams.cam0_ppm_y),
				   3, cv::Scalar(0,0,220), cv::FILLED);

		for (auto match : matchMap) {
			cv::Scalar clr = utl::randColor();

			cv::Rect lBox = lYOLOBoxes.at(match.first).box;
			cv::Rect rBox = rYOLOBoxes.at(match.second).box;


			if (lBox.x < lScene_gry.cols/3 || lBox.x+lBox.width > lScene_gry.cols*2/3) continue;


			Vector2d lMidPoint((lBox.x + lBox.width*0.5) / stereoParams.cam0_ppm_x, (lBox.y + lBox.height*0.5) / stereoParams.cam0_ppm_y);
			Vector2d rMidPoint((rBox.x + rBox.width*0.5) / stereoParams.cam1_ppm_x, (rBox.y + rBox.height*0.5) / stereoParams.cam1_ppm_y);

			ImageCoord imageCoordL(lMidPoint, stereoParams.cam0_intrinsic);
			ImageCoord imageCoordR(rMidPoint, stereoParams.cam1_intrinsic);

			std::vector<cv::Point2f> pts_1, pts_2;
			cv::Mat pts_4d;

			pts_1.push_back(cv::Point2f(imageCoordL.X(), imageCoordL.Y()));
			pts_2.push_back(cv::Point2f(imageCoordR.X(), imageCoordR.Y()));
			cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);
			cv::Mat x = pts_4d.col(0);
			x /= x.at<float>(3, 0);
			cv::Point3d p(
					x.at<float>(0, 0),
					x.at<float>(1, 0),
					x.at<float>(2, 0)
			);

			Eigen::Vector3d groundPos = ForwardIntersection::calc(coResult, imageCoordL, imageCoordR);

			std::stringstream ss;
			ss << std::fixed << std::setprecision(2)
			<< p.x << ","
			<< p.y << ","
			<< p.z << ".";

			cv::Mat hsv, bgr;
			cv::cvtColor(cv::Mat(1, 1, CV_8UC3, clr), hsv, cv::COLOR_BGR2HSV);
			cv::Vec3b hsv_clr = hsv.at<cv::Vec3b>(0, 0);
			hsv_clr[1] = ( abs(1-abs(p.z)/50) < 1 ? abs(1-abs(p.z)/50) : 1 ) * 255;

			cv::cvtColor(cv::Mat(1, 1, CV_8UC3, hsv_clr), bgr, cv::COLOR_HSV2BGR);
			clr = cv::Scalar(bgr.at<cv::Vec3b>(0, 0));

			if (lBox.x > lScene_gry.cols*0.5) {
				utl::drawText(ss.str(), canvas, lBox.br(), cv::Scalar(0, 0, 0), clr, 0.5, utl::TextAnchor::TOP_LEFT);
			} else {
				utl::drawText(ss.str(), canvas, lBox.tl(), cv::Scalar(0, 0, 0), clr, 0.5, utl::TextAnchor::BOTTOM_RIGHT);
			}

			cv::rectangle(canvas, lBox, clr, 2);

			std::cout << ss.str() << std::endl;
		}

		cv::imshow("MAIN", canvas);
		cv::waitKey(1);
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}

	return 0;
}
